#include "rclcpp/rclcpp.hpp"
#include "common/srv/add_two_ints.hpp"

#include <chrono>
#include <cstdlib>
#include <memory>

using namespace std::chrono_literals;

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("add_two_ints_client");
    rclcpp::Client<common::srv::AddTwoInts>::SharedPtr add_client = node->create_client<common::srv::AddTwoInts>("add_two_ints");
    while (!add_client->wait_for_service(1s)) {
        if (!rclcpp::ok()) {
            RCLCPP_ERROR(node->get_logger(), "add_two_ints service not available");
            return -1;
        }
        RCLCPP_INFO(node->get_logger(), "waiting for add_two_ints service");
    }

    auto req = std::make_shared<common::srv::AddTwoInts::Request>();
    req->a = 1;
    req->b = 2;

    auto ret = add_client->async_send_request(req);
    if (rclcpp::spin_until_future_complete(node, ret) == rclcpp::FutureReturnCode::SUCCESS)
    {
        RCLCPP_INFO(node->get_logger(), "Result of add: %ld", ret.get()->sum);
    }
    else
    {
        RCLCPP_ERROR(node->get_logger(), "Failed to call add_two_ints service");
    }

    // 多次请求
    int count = 5;
    while (count--)
    {
        req->a = count;
        req->b = 2 * count;
        ret = add_client->async_send_request(req);

        if (rclcpp::spin_until_future_complete(node, ret) == rclcpp::FutureReturnCode::SUCCESS)
        {
            RCLCPP_INFO(node->get_logger(), "Result of add: %ld + %ld = %ld", req->a, req->b, ret.get()->sum);
        }
        else
        {
            RCLCPP_ERROR(node->get_logger(), "Failed to call add_two_ints service");
        }
    }

    rclcpp::shutdown();
    return 0;
}